#include "socket_can_node.h"
SocketCanNode::SocketCanNode(std::string can_name, double rate) : loop_rate_(rate)
{
	socket_can_ptr_ = std::make_unique<socket_can::SocketCAN>(can_name);
	//int _foodPerTurn, int _hungerPerTurn, int _hungerMin,\
    //int _criticalPoint, int _hungerMax)
	control_cmd_dog_.setAllParameter(8, 1, 0, 17, 25);
	chassis_can_dog_.setAllParameter(8, 1, 0, 17, 25);
	std::string folderpath = can_dbcs_path;
	std::vector<std::string> files;
	getAllFilesInFolder(folderpath, &files);
	for (std::string &file : files)
	{
		std::cout << file << std::endl;
		dbc_analysis::DbcAnalysis::getInstance()->addOneDbcFile(file);
	}
	dbc_analysis::DbcAnalysis::getInstance()->analysisFiles();
	dbc_analysis::DbcAnalysis::getInstance()->printMessages();
	can_msgs_pool_.init(dbc_analysis::DbcAnalysis::getInstance()->getMessages());
	std::function < void(can_frame) can_cb = ;
	std::function<void(can_frame)> can_cb_localizition_fun = std::bind(&SocketCanNode::can_cb_localizition, this,
																	   std::placeholders::_1);
	socket_can_ptr_->register_asyn_read_cb(can_cb_localizition_fun);
	std::function<void(can_frame)> can_cb_chassis_fun = std::bind(&SocketCanNode::can_cb_chassis, this,
																  std::placeholders::_1);
	socket_can_ptr_->register_asyn_read_cb(can_cb_chassis_fun);
}
//position heading and velocity
void SocketCanNode::can_cb_localizition(can_frame frame)
{

	/*if (frame.can_id == 0x703) //latitude and longtitude
	{
		std::vector<double> valueOut1(2);
		can_util::unpackCanmsg(dbc_analysis::DbcAnalysis::getInstance()
								   ->getMessages()[frame.can_id],
							   frame, valueOut1);
		latitude_ = valueOut1[0];
		longtitude_ = valueOut1[1];
		std::cout << "latitude_ is:" << latitude_ << std::endl;
		std::cout << "longtitude_ is:" << longtitude_ << std::endl;
	}
	else if (frame.can_id == 0x704) //altitude
	{
		std::vector<double> valueOut1(2);
		can_util::unpackCanmsg(dbc_analysis::DbcAnalysis::getInstance()
								   ->getMessages()[frame.can_id],
							   frame, valueOut1);
		altitude_ = valueOut1[0]; //
		std::cout << "altitude_ is:" << altitude_ << std::endl;
	}
	else if (frame.can_id == 0x705) //position local
	{
		std::vector<double> valueOut1(4);
		can_util::unpackCanmsg(dbc_analysis::DbcAnalysis::getInstance()
								   ->getMessages()[frame.can_id],
							   frame, valueOut1);
		localizition_.vX = valueOut1[0];
		localizition_.vY = valueOut1[1];
		std::cout << "local x is:" << valueOut1[0] << std::endl;
		std::cout << "local y is:" << valueOut1[1] << std::endl;
		localizition_.z = 0.0;
	}*/
}
void SocketCanNode::can_cb_chassis(can_frame frame)
{

	if (frame.can_id == 0x403) //abs speed
	{
		chassis_can_dog_.giveFood();
		std::vector<double> valueOut1(1);
		can_util::unpackCanmsg(dbc_analysis::DbcAnalysis::getInstance()
								   ->getMessages()[frame.can_id],
							   frame, valueOut1);
		chassis_.speed_mps = valueOut1[0] / 3.6;
		std::cout << "vehicle speed_mps is:" << chassis_.speed_mps << std::endl;
	}
	if (frame.can_id == 0x394) //tire angle
	{
		std::vector<double> valueOut1(2);
		can_util::unpackCanmsg(dbc_analysis::DbcAnalysis::getInstance()
								   ->getMessages()[frame.can_id],
							   frame, valueOut1);
		chassis_.steering_angle_f = valueOut1[0];
		chassis_.steering_angle_r = 0;
		std::cout << "tire f angle(degree) is:" << chassis_.steering_angle_f << std::endl;
	}
}
void SocketCanNode::cb_control_cmd(const auto_msgs::control_cmd &msg)
{
	control_cmd_ = msg;
	control_cmd_dog_.giveFood();
}
void SocketCanNode::no_food()
{
	chassis_can_dog_.noFood();
	control_cmd_dog_.noFood();
}
bool SocketCanNode::check_dog()
{

	if (control_cmd_dog_.isDied())
	{
		ROS_ERROR("control cmd is time out");
		return false;
	}
	else
	{
		return true;
	}
}
void SocketCanNode::hard_brake_to_can()
{
	//control_cmd_to_can(0.0, 5.0, 0.0);
}
void SocketCanNode::control_cmd_to_can(double acc_torque, double decelaration, double tire_angle)
{
	//************************pack 0xa0 msgs
	std::vector<double> signals_value_0a0(10, 0);
	//live counter
	if (live_counter_0a0_ < 15)
	{
		live_counter_0a0_ += 1;
	}
	else
	{
		live_counter_0a0_ = 0;
	}

	signals_value_0a0[0] = 0.0;		   //check_sum bcc
	signals_value_0a0[1] = 0.0;		   //error_status:0x00 no error,0x01 derate,0x03 error
	signals_value_0a0[2] = acc_torque; //torque_request
	signals_value_0a0[3] = live_counter_0a0_;
	signals_value_0a0[4] = 2.0;			 // acc_status:0x00 off, 0x01 stand by, 0x02 active, 0x03 override, 0x04 shut off
	signals_value_0a0[5] = 0.0;			 //standstill_request:0x00 No_request,0x01 Request
	signals_value_0a0[6] = 0.0;			 //prefill_brake_request: 0x00 No_request,prefilling requested
	signals_value_0a0[7] = decelaration; //decelerate_request
	signals_value_0a0[8] = 1.0;			 //status_acc_deceleration_request:0x00 No Request,0x01 non_emergency_brake,0x02 emergency_brake
	signals_value_0a0[9] = 1.0;			 //status_torque_request:0x00 No_request, 0x01 Torque_request 0x03 Invalid Value
	can_frame frame_010 = {0};
	//int packCanmsg(const Message &m, const std::vector<double> &signals_value, can_frame &msg);
	can_util::packCanmsg(dbc_analysis::DbcAnalysis::getInstance()->getMessages()[0x0a0], signals_value_0a0, frame_010);
	//cheack sum 0x0a0
	frame_010.data[0] = 0x00;
	for (int i = 1; i < 8; i++)
	{
		frame_010.data[0] ^= frame_010.data[i];
	}
	socket_can_ptr_->write_frame(frame_010);
	//************************pack 0x130 msgs
	can_frame frame_130 = {0};
	//live counter
	if (live_counter_130_ < 15)
	{
		live_counter_130_ += 1;
	}
	else
	{
		live_counter_130_ = 0;
	}
}
void SocketCanNode::speed_to_m2(double speed_kmh, int gear)
{
	std::vector<double> signals_speed_to_m2(3, 0);
	can_frame frame_x099 = {0};
	signals_speed_to_m2[0] = gear;
	signals_speed_to_m2[1] = std::abs(speed_kmh);
	can_util::packCanmsg(dbc_analysis::DbcAnalysis::getInstance()->getMessages()[0x099],
						 signals_speed_to_m2, frame_x099);
	unsigned char sum = 0;
	for (int i = 0; i <= 6; i++)
	{
		sum += frame_x099.data[i];
	}
	unsigned char check_sum = sum ^ 0xff;
	frame_x099.data[7] = check_sum;
	socket_can_ptr_->write_frame(frame_x099);
}
void SocketCanNode::step()
{
	no_food();
	if (control_cmd_dog_.isDied())
	{
		hard_brake_to_can();
		ROS_ERROR("control cmd is time out");
	}
	if (chassis_can_dog_.isDied())
	{
		ROS_ERROR("SocketCanNode::chassis_can_dog_ is time out");
	}
	else
	{
		speed_to_m2(chassis_.speed_mps * 3.6, 0);
	}
	speed_to_m2(10 * 3.6, 0);
	//localizition_pub_.publish(localizition_);
	//chassis_pub_.publish(chassis_);
	ros::spinOnce(); //可用回调函数
	loop_rate_.sleep();
}